﻿/*
	Copyright (c) 2013 Andrew Meyer

	Permission is hereby granted, free of charge, to any person obtaining a copy of this software
	and associated documentation files (the "Software"), to deal in the Software without
	restriction, including without limitation the right to use, copy, modify, merge, publish,
	distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
	Software is furnished to do so, subject to the following conditions:

	The above copyright notice and this permission notice shall be included in all copies or
	substantial portions of the Software.

	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING   
	BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
	NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
	DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

using System.Windows.Media.Media3D;

namespace Ascend
{
	/*
	 * HomogeneousTransform represents a 4x4 matrix of the form
	 * 
	 *		H = [ M  B ]
     *			[ 0  1 ]
	 *
	 *	where
	 *	
	 *		M = [ M11  M12  M13 ]
	 *			[ M21  M22  M23 ]
	 *			[ M31  M32  M33 ]
	 *	
	 *	and
	 *	
	 *		B = [ B1 ]
	 *			[ B2 ]
	 *			[ B3 ]
	 */
	internal class HomogeneousTransform
	{
		#region Lifecycle
		public HomogeneousTransform()
		{ }

		public HomogeneousTransform(double m11, double m12, double m13, double b1,
									double m21, double m22, double m23, double b2,
									double m31, double m32, double m33, double b3)
		{
			M11 = m11;
			M12 = m12;
			M13 = m13;
			M21 = m21;
			M22 = m22;
			M23 = m23;
			M31 = m31;
			M32 = m32;
			M33 = m33;
			B1 = b1;
			B2 = b2;
			B3 = b3;
		}

		// Matrix3D stores its values transposed relative to this class
		public HomogeneousTransform(Matrix3D matrix)
			: this(matrix.M11, matrix.M21, matrix.M31, matrix.OffsetX,
				   matrix.M12, matrix.M22, matrix.M32, matrix.OffsetY,
				   matrix.M13, matrix.M23, matrix.M33, matrix.OffsetZ)
		{ }

		public HomogeneousTransform(ObjectPose pose)
			: this(PoseToMatrix(pose))
		{ }
		#endregion

		#region Public Members
		public double M11 = 1.0;
		public double M12 = 0.0;
		public double M13 = 0.0;
		public double M21 = 0.0;
		public double M22 = 1.0;
		public double M23 = 0.0;
		public double M31 = 0.0;
		public double M32 = 0.0;
		public double M33 = 1.0;
		public double B1 = 0.0;
		public double B2 = 0.0;
		public double B3 = 0.0;
		#endregion

		#region Operator Overloads
		public static HomogeneousTransform operator +(HomogeneousTransform lhs,
			HomogeneousTransform rhs)
		{
			return new HomogeneousTransform(
				lhs.M11 + rhs.M11, lhs.M12 + rhs.M12, lhs.M13 + rhs.M13, lhs.B1 + rhs.B1,
				lhs.M21 + rhs.M21, lhs.M22 + rhs.M22, lhs.M23 + rhs.M23, lhs.B2 + rhs.B2,
				lhs.M31 + rhs.M31, lhs.M32 + rhs.M32, lhs.M33 + rhs.M33, lhs.B3 + rhs.B3);
		}

		public static HomogeneousTransform operator *(HomogeneousTransform lhs,
			HomogeneousTransform rhs)
		{
			return new HomogeneousTransform(
				lhs.M11 * rhs.M11 + lhs.M12 * rhs.M21 + lhs.M13 * rhs.M31,
				lhs.M11 * rhs.M12 + lhs.M12 * rhs.M22 + lhs.M13 * rhs.M32,
				lhs.M11 * rhs.M13 + lhs.M12 * rhs.M23 + lhs.M13 * rhs.M33,
				lhs.M11 * rhs.B1  + lhs.M12 * rhs.B2  + lhs.M13 * rhs.B3  + lhs.B1,

				lhs.M21 * rhs.M11 + lhs.M22 * rhs.M21 + lhs.M23 * rhs.M31,
				lhs.M21 * rhs.M12 + lhs.M22 * rhs.M22 + lhs.M23 * rhs.M32,
				lhs.M21 * rhs.M13 + lhs.M22 * rhs.M23 + lhs.M23 * rhs.M33,
				lhs.M21 * rhs.B1  + lhs.M22 * rhs.B2  + lhs.M23 * rhs.B3  + lhs.B2,

				lhs.M31 * rhs.M11 + lhs.M32 * rhs.M21 + lhs.M33 * rhs.M31,
				lhs.M31 * rhs.M12 + lhs.M32 * rhs.M22 + lhs.M33 * rhs.M32,
				lhs.M31 * rhs.M13 + lhs.M32 * rhs.M23 + lhs.M33 * rhs.M33,
				lhs.M31 * rhs.B1  + lhs.M32 * rhs.B2  + lhs.M33 * rhs.B3  + lhs.B3);
		}

		public static Point3D operator *(HomogeneousTransform lhs, Point3D rhs)
		{
			return new Point3D(
				lhs.M11 * rhs.X + lhs.M12 * rhs.Y + lhs.M13 * rhs.Z + lhs.B1,
				lhs.M21 * rhs.X + lhs.M22 * rhs.Y + lhs.M23 * rhs.Z + lhs.B2,
				lhs.M31 * rhs.X + lhs.M32 * rhs.Y + lhs.M33 * rhs.Z + lhs.B3);
		}

		public static Vector3D operator *(HomogeneousTransform lhs, Vector3D rhs)
		{
			return new Vector3D(
				lhs.M11 * rhs.X + lhs.M12 * rhs.Y + lhs.M13 * rhs.Z,
				lhs.M21 * rhs.X + lhs.M22 * rhs.Y + lhs.M23 * rhs.Z,
				lhs.M31 * rhs.X + lhs.M32 * rhs.Y + lhs.M33 * rhs.Z);
		}

		public static HomogeneousTransform operator *(HomogeneousTransform lhs, double rhs)
		{
			return new HomogeneousTransform(
				lhs.M11 * rhs, lhs.M12 * rhs, lhs.M13 * rhs, lhs.B1 * rhs,
				lhs.M21 * rhs, lhs.M22 * rhs, lhs.M23 * rhs, lhs.B2 * rhs,
				lhs.M31 * rhs, lhs.M32 * rhs, lhs.M33 * rhs, lhs.B3 * rhs);
		}

		public static HomogeneousTransform operator /(HomogeneousTransform lhs, double rhs)
		{
			return new HomogeneousTransform(
				lhs.M11 / rhs, lhs.M12 / rhs, lhs.M13 / rhs, lhs.B1 / rhs,
				lhs.M21 / rhs, lhs.M22 / rhs, lhs.M23 / rhs, lhs.B2 / rhs,
				lhs.M31 / rhs, lhs.M32 / rhs, lhs.M33 / rhs, lhs.B3 / rhs);
		}
		#endregion

		#region Public Properties
		public HomogeneousTransform Inverse
		{
			get
			{
				// Compute inverse(M)
				double n11 = M22 * M33 - M23 * M32;
				double n12 = M13 * M32 - M12 * M33;
				double n13 = M12 * M23 - M13 * M22;
				double n21 = M23 * M31 - M21 * M33;
				double n22 = M11 * M33 - M13 * M31;
				double n23 = M13 * M21 - M11 * M23;
				double n31 = M21 * M32 - M22 * M31;
				double n32 = M12 * M31 - M11 * M32;
				double n33 = M11 * M22 - M12 * M21;

				double mDet = M13 * n31 + M12 * n21 + M11 * n11;

				if (mDet == 0)
				{
					return null;
				}

				double mInv11 = n11 / mDet;
				double mInv12 = n12 / mDet;
				double mInv13 = n13 / mDet;
				double mInv21 = n21 / mDet;
				double mInv22 = n22 / mDet;
				double mInv23 = n23 / mDet;
				double mInv31 = n31 / mDet;
				double mInv32 = n32 / mDet;
				double mInv33 = n33 / mDet;

				// Compute r = -inverse(M) * B
				double r1 = -(mInv11 * B1 + mInv12 * B2 + mInv13 * B3);
				double r2 = -(mInv21 * B1 + mInv22 * B2 + mInv23 * B3);
				double r3 = -(mInv31 * B1 + mInv32 * B2 + mInv33 * B3);
				
				// Return inverse(H) = [ inverse(M)  r ]
				//					   [     0       1 ]
				return new HomogeneousTransform(mInv11, mInv12, mInv13, r1,
												mInv21, mInv22, mInv23, r2,
												mInv31, mInv32, mInv33, r3);
			}
		}
		#endregion

		#region Private Methods
		private static Matrix3D PoseToMatrix(ObjectPose pose)
		{
			// Setup translation and scale transforms
			TranslateTransform3D translateTransform =
				new TranslateTransform3D(pose.X, pose.Y, pose.Z);
			ScaleTransform3D scaleTransform =
				new ScaleTransform3D(pose.ScaleX, pose.ScaleY, pose.ScaleZ);

			// Setup rotation transform using YZX Euler angles
			Transform3DGroup rotateTransform = new Transform3DGroup();
			rotateTransform.Children.Add(new RotateTransform3D(
				new AxisAngleRotation3D(new Vector3D(0, 1, 0), pose.AngleY))); // yaw
			rotateTransform.Children.Add(new RotateTransform3D(
				new AxisAngleRotation3D(new Vector3D(0, 0, 1), pose.AngleZ))); // pitch
			rotateTransform.Children.Add(new RotateTransform3D(
				new AxisAngleRotation3D(new Vector3D(1, 0, 0), pose.AngleX))); // roll

			// Setup total transform (order of children matters)
			Transform3DGroup trans = new Transform3DGroup();
			trans.Children.Add(scaleTransform);
			trans.Children.Add(rotateTransform);
			trans.Children.Add(translateTransform);

			return trans.Value;
		}
		#endregion
	}
}
